
/*========================================================================================
 *                                                                                      *
 *               Copyright(c) 2013-2014, Cha SangWoo, All rights reserved.              *
 *                                                                                      *
 * This file is part of ChaOS.                                                          *
 *                                                                                      *
 * Permission to use, copy, modify, and distribute this software in source and binary   *
 * forms and its documentation for any purpose and without fee is hereby granted,       *
 * provided that the above copyright notice appear in all copies and that both that     *
 * copyright notice and this permission notice appear in supporting documentation.      *
 *                                                                                      *
 * THIS SOFTWARE IS PROVIDED BY THE CHA SANGWOO AND CONTRIBUTORS "AS IS" AND ANY        *
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED            *
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.   *
 * IN NO EVENT SHALL CHA SANGWOO OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,    *
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         *
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,          *
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF            *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE      *
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,                    *
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                   *
 *                                                                                      *
========================================================================================*/

#include "co_core.h"
#include "co_queue.h"
#include "co_semaphore.h"

#if (FLAG_USE_CHAOS_QUEUE == CO_ENABLE)

#ifdef __cplusplus
    namespace ChaOS {
#endif

//////////////////////////////////////  < BEGIN >  ///////////////////////////////////////

/*======================================================================================*/
/*!
    @fn             VOID _co_queue_memcpy(UINT8 *pdst, UINT8 *psrc, UINT size)

    @brief          

    @param [in]     pdst - 
    @param [in]     psrc - 
    @param [in]     size - 

    @return         None

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
VOID _co_queue_memcpy(UINT8 *pdst, UINT8 *psrc, UINT size)
{
    for (; size >= sizeof(UINT); size-=4, pdst+=4, psrc+=4)
    {
        *((UINT32*)pdst) = *((UINT32*)psrc);
    }

    for (; size > 0; size--, pdst++, psrc++)
    {
        *pdst = *psrc;
    }
}


/*======================================================================================*/
/*!
    @fn             OBJECT co_queue_create(UINT item_size, UINT item_count)

    @brief          

    @param [in]     item_size  - 
    @param [in]     item_count - 

    @return         Object for queue

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
OBJECT co_queue_create(UINT item_size, UINT item_count)
{
    QUEUE_CB   *pqueue;
    UINT        size;

    CO_ASSERT(item_size > 0);
    CO_ASSERT(item_count > 0);

#if (FLAG_USE_QUEUE_ITEM_ALIGN == CO_ENABLE)
    size  = (UINT)SIZE_ALIGN(item_size, SIZE_QUEUE_ITEM_ALIGN);
#else
    size  = item_size;
#endif

    size *= item_count;
    size += sizeof(QUEUE_CB) - sizeof(UINT32);

    // allocate heap memory
    pqueue = (QUEUE_CB*) co_heap_alloc(size);
    CO_ASSERT(pqueue);

    // create semaphore for receiving
    pqueue->objRecv = co_semaphore_create(0, item_count);
    CO_ASSERT(pqueue->objRecv);

    // create semaphore for sending
    pqueue->objSend = co_semaphore_create(item_count, item_count);
    CO_ASSERT(pqueue->objSend);

    // set object ID
    OBJID_SET(pqueue, OBJ_QUEUE_ID);

    // initialize
    pqueue->item_size   = item_size;
    pqueue->item_count  = item_count;

    pqueue->read_pos  = 0;
    pqueue->write_pos = 0;
    pqueue->max_size  = size - sizeof(QUEUE_CB) - sizeof(UINT32);

    return (OBJECT) pqueue;
}


/*======================================================================================*/
/*!
    @fn             CO_RESULT co_queue_send(OBJECT object, VOID *pitem, BOOL overwrite)

    @brief          

    @param [in]     object - Object for queue
    @param [in]     pitem  - 

    @return         None

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
CO_RESULT co_queue_send(OBJECT object, VOID *pitem, UINT16 timeout)
{
    QUEUE_CB   *pqueue = (QUEUE_CB*) object;
    UINT8      *pdst;

    // check object ID
    OBJID_CHECK(pqueue, OBJ_QUEUE_ID);

    // check is not full and overwrite
    if (co_semaphore_take(pqueue->objSend, timeout) == CO_OK_SUCCESS)
    {
        ///////////////////
        CO_ENTER_CRITICAL();

        pdst = (UINT8*)((UINT)pqueue->start + pqueue->write_pos);

        _co_queue_memcpy(pdst, (UINT8*)pitem, pqueue->item_size);

        pqueue->write_pos += pqueue->item_size;
        if (pqueue->write_pos >= pqueue->max_size)
            pqueue->write_pos = 0;

        CO_LEAVE_CRITICAL();
        ////////////////////

        co_semaphore_give(pqueue->objRecv);

        return (CO_RESULT) CO_OK_SUCCESS;
    }

    return (CO_RESULT) CO_NG_QUEUE_FULL;
}


/*======================================================================================*/
/*!
    @fn             CO_RESULT co_queue_recv(OBJECT object, VOID *pitem, UINT16 timeout)

    @brief          

    @param [in]     object  - Object for queue
    @param [in]     pitem   - 
    @param [in]     timeout - 

    @return         CO_RESULT value

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
CO_RESULT co_queue_recv(OBJECT object, VOID *pitem, UINT16 timeout)
{
    QUEUE_CB   *pqueue = (QUEUE_CB*) object;
    UINT8      *psrc;

    // check object ID
    OBJID_CHECK(pqueue, OBJ_QUEUE_ID);

    if (co_semaphore_take(pqueue->objRecv, timeout) == CO_OK_SUCCESS)
    {
        ///////////////////
        CO_ENTER_CRITICAL();

        psrc = (UINT8*)((UINT)pqueue->start + pqueue->read_pos);

        _co_queue_memcpy((UINT8*)pitem, psrc, pqueue->item_size);

        pqueue->read_pos += pqueue->item_size;
        if (pqueue->read_pos >= pqueue->max_size)
            pqueue->read_pos = 0;

        CO_LEAVE_CRITICAL();
        ////////////////////

        co_semaphore_give(pqueue->objSend);

        return (CO_RESULT) CO_OK_SUCCESS;
    }

    return (CO_RESULT) CO_NG_QUEUE_EMPTY;
}


/*======================================================================================*/
/*!
    @fn             UINT16 co_queue_get_item_size(OBJECT object)

    @brief          Get size of item in queue

    @param [in]     object  - Object for queue

    @return         Size of item in queue

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
UINT16 co_queue_get_item_size(OBJECT object)
{
    QUEUE_CB   *pqueue = (QUEUE_CB*) object;

    // check object ID
    OBJID_CHECK(pqueue, OBJ_QUEUE_ID);

    return pqueue->item_size;
}


/*======================================================================================*/
/*!
    @fn             UINT16 co_queue_get_item_count(OBJECT object)

    @brief          Get count of items in queue

    @param [in]     object - Object for mutex

    @return         Count of items in queue

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
UINT16 co_queue_get_item_count(OBJECT object)
{
    QUEUE_CB   *pqueue = (QUEUE_CB*) object;

    // check object ID
    OBJID_CHECK(pqueue, OBJ_QUEUE_ID);

    return co_semaphore_get_cur_count(pqueue->objRecv);
}


/*======================================================================================*/
/*!
    @fn             VOID co_queue_flush(OBJECT object)

    @brief          Clear queue. The items in queue will be delete.

    @param [in]     object - Object for queue

    @return         None

    - HISTORY -
    [V00.00.01] 2014-03-23 [aireat] - Created.
*/
/*======================================================================================*/
VOID co_queue_flush(OBJECT object)
{
    QUEUE_CB     *pqueue = (QUEUE_CB*) object;

    // check object ID
    OBJID_CHECK(pqueue, OBJ_QUEUE_ID);

    ///////////////////
    CO_ENTER_CRITICAL();

    co_semaphore_reset_count(pqueue->objSend);
    co_semaphore_add_count(pqueue->objSend, pqueue->item_count);
    co_semaphore_reset_count(pqueue->objRecv);
    pqueue->read_pos  = 0;
    pqueue->write_pos = 0;

    CO_LEAVE_CRITICAL();
    ////////////////////
}


//////////////////////////////////////  <  END  >  ///////////////////////////////////////

#ifdef __cplusplus
    } /* namespace ChaOS */
#endif

#endif // (FLAG_USE_CHAOS_QUEUE == CO_ENABLE)

